#define A_ECHO 8  // echo接3端口

//#define A_TRIG 53  // trig接2端口
//#define B_TRIG 52
//#define C_TRIG 51
//#define D_TRIG 50
//#define E_TRIG 49
//#define F_TRIG 48
//#define ALARM_DISTANCE_A 20

#define Ultrasonic_Num 5

const unsigned char trig_pin_num[6]={53,52,51,50,49,48};
float distance_of_ultrasonic[Ultrasonic_Num];                  //if it is 0 means the distance is far enough,but it is not 0 means the real distance
unsigned char Ultrasonic_Ser=0;                                //it is used to switch between the diff Ultrasonic 

void init_distance_of_ultrasonic()
{
  char i;
  for(i=0;i<Ultrasonic_Num;i++)
   distance_of_ultrasonic[i]=0;
  Ultrasonic_Ser=0;
}
void hc_sr04_pin_init()
{
  char i;
  pinMode(A_ECHO,INPUT);
  for(i=0;i<Ultrasonic_Num;i++)
    pinMode(trig_pin_num[i],OUTPUT);
  //pinMode(A_TRIG,OUTPUT);  
}
float trig_measure_of_a(unsigned char ser)
{
  float distance;
  
  //digitalWrite(trig_pin_num[ser],LOW);
  digitalWrite(trig_pin_num[ser],LOW);
  delayMicroseconds(2);
  digitalWrite(trig_pin_num[ser],HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin_num[ser],LOW);

  distance = pulseIn(A_ECHO,HIGH,5000);
  distance=distance/58.0;

  /*
  //Serial.print(distance);
  //Serial.println("cm");
  //delay(60);
  if(distance==0)
   return 0;
  if(distance<ALARM_DISTANCE_A)
  {
   setMotorSpeeds(0,0);
   //Serial.println("stop");
   a=1;
  }
  return a;
  */
  return distance;
}
void switch_the_Ultrasonic_Num(void)
{
  Ultrasonic_Ser+=1;
  if(Ultrasonic_Ser>=Ultrasonic_Num)
   Ultrasonic_Ser=0;
}
/*
void get_the_real_distance_of_Ultrasonic(void)
{
  int i;
  switch_the_Ultrasonic_Num();
  distance_of_ultrasonic[Ultrasonic_Ser]=trig_measure_of_a(Ultrasonic_Ser);
  /*
  for(i=0;i<Ultrasonic_Num;i++)
  {
    Serial.print(i);
    Serial.print("   ----   "); 
    Serial.print(distance_of_ultrasonic[i]);
    Serial.println("cm\r\n");
  }
  delay(500);
  //
}
*/
void get_the_real_distance_of_Ultrasonic(void)
{
  /*
  int i;
  for(i=0;i<Ultrasonic_Num;i++)
  {
    distance_of_ultrasonic[i]=trig_measure_of_a(i);
    Serial.print(i);
    Serial.print("   ----   "); 
    Serial.print(distance_of_ultrasonic[i]);
    Serial.println("cm\r\n");
    delay(60);
  }
  */
  switch_the_Ultrasonic_Num();
  distance_of_ultrasonic[Ultrasonic_Ser]=trig_measure_of_a(Ultrasonic_Ser);
}

void report_to_ros_of_Ultrasonic(void)
{
  int i;
  int j;
  for(i=0;i<Ultrasonic_Num;i++)
  {
    j=distance_of_ultrasonic[i]*100;
    //Serial.print(distance_of_ultrasonic[i]);
    if(i==(Ultrasonic_Num-1))
    {
         Serial.println(j);
    }
    else
    {
        Serial.print(j);
        Serial.print(" ");
    }
  }
}

